{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.6.9-final"
    },
    "colab": {
      "name": "Robotic Manipulation - Lecture 4 Breakout.ipynb",
      "provenance": [],
      "collapsed_sections": [],
      "toc_visible": true
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "EgiF12Hf1Dhs",
        "colab_type": "text"
      },
      "source": [
        "This notebook is meant for our lecture 4 breakout session.  I've put together some questions to guide your [here](https://itempool.com/MIT-Robotic-Manipulation/c/yzVbTp9zFc_).  The points are not real, and will not contribute to your grade!\n",
        "\n",
        "# Notebook setup\n",
        "\n",
        "The following cell will:\n",
        "- on Colab (only), install Drake to `/opt/drake`, install Drake's prerequisites via `apt`, and add pydrake to `sys.path`.  This will take approximately two minutes on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  If you navigate between notebooks using Colab's \"File->Open\" menu, then you can avoid provisioning a separate machine for each notebook.\n",
        "- launch a server for our 3D visualizer (MeshCat) that will be used for the remainder of this notebook.\n",
        "\n",
        "You will need to rerun this cell if you restart the kernel, but it should be fast because the machine will already have drake installed."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "eeMrMI0-1Dhu",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "import importlib\n",
        "import sys\n",
        "from urllib.request import urlretrieve\n",
        "\n",
        "# Install drake.\n",
        "if 'google.colab' in sys.modules and importlib.util.find_spec('pydrake') is None:\n",
        "  version='20200918'\n",
        "  build='nightly'\n",
        "  urlretrieve(f\"https://drake-packages.csail.mit.edu/drake/{build}/drake-{version}/setup_drake_colab.py\",\n",
        "              \"setup_drake_colab.py\")\n",
        "  from setup_drake_colab import setup_drake\n",
        "  setup_drake(version=version, build=build)\n",
        "  !pip install pyngrok==4.2.2\n",
        "\n",
        "# Install pyngrok.\n",
        "server_args = []\n",
        "if 'google.colab' in sys.modules:\n",
        "  server_args = ['--ngrok_http_tunnel']\n",
        "\n",
        "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
        "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
        "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
        "\n",
        "# Determine if this notebook is currently running as a notebook or a unit test.\n",
        "from IPython import get_ipython\n",
        "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
        "\n",
        "# Let's do all of our imports here, too.\n",
        "import numpy as np\n",
        "import altair as alt\n",
        "import pydot\n",
        "from IPython.display import display, SVG\n",
        "from ipywidgets import Layout, Text, Textarea, ToggleButton, ToggleButtons\n",
        "\n",
        "from pydrake.all import (\n",
        "    AddMultibodyPlantSceneGraph, AngleAxis, BasicVector, ConnectMeshcatVisualizer, \n",
        "    DiagramBuilder, FindResourceOrThrow, Integrator, JacobianWrtVariable, \n",
        "    LeafSystem, MultibodyPlant, MultibodyPositionToGeometryPose, Parser, \n",
        "    PiecewisePolynomial, PiecewiseQuaternionSlerp, Quaternion, RigidTransform, \n",
        "    RollPitchYaw, RotationMatrix, SceneGraph, Simulator, TrajectorySource,\n",
        "    DifferentialInverseKinematicsParameters, DifferentialInverseKinematicsIntegrator,\n",
        "    set_log_level\n",
        ")\n",
        "from pydrake.examples.manipulation_station import ManipulationStation\n",
        "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback\n",
        "from pydrake.systems.jupyter_widgets import PoseSliders, WidgetSystem"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "hSeD3PBotJUU",
        "colab_type": "text"
      },
      "source": [
        "# Kinematic Jacobians for pick and place\n",
        "\n",
        "Let's set up the same iiwa + wsg example, with sliders (but without the frames), that we used above.  But this time I'll display the value of the Jacobian $J^G(q)$."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "6F_KZoc1QDkY",
        "colab_type": "code",
        "tags": [],
        "colab": {}
      },
      "source": [
        "def pick_and_place_jacobians_example():\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    # TODO: Replace this with a simple model directive of iiwa+wsg (no clutter bins)\n",
        "    station = builder.AddSystem(ManipulationStation())\n",
        "    station.SetupClutterClearingStation()\n",
        "    station.Finalize()\n",
        "\n",
        "    frames_to_draw = {\"gripper\": {\"body\"}}\n",
        "    meshcat = ConnectMeshcatVisualizer(builder,\n",
        "        station.get_scene_graph(),\n",
        "        output_port=station.GetOutputPort(\"pose_bundle\"),\n",
        "        delete_prefix_on_load=False,\n",
        "        zmq_url=zmq_url,\n",
        "        frames_to_draw=frames_to_draw,\n",
        "        axis_length=0.2,\n",
        "        axis_radius=0.005)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "\n",
        "    jacobian = Textarea(value=\"\", description=\"J_G: \", layout={'width':'60%','height':'100px'}, style={'description_width':'initial'})\n",
        "    sigma = Text(value=\"\", description=\"smallest singular value(J_G): \", layout={'width':'60%'}, style={'description_width':'initial'})\n",
        "    plant = station.get_multibody_plant()\n",
        "\n",
        "    G = plant.GetBodyByName(\"body\").body_frame()\n",
        "    W = plant.world_frame()\n",
        "    def pose_callback(context):\n",
        "        J_G = plant.CalcJacobianSpatialVelocity(context, JacobianWrtVariable.kQDot, G, [0,0,0], W, W)   ## This is the important line\n",
        "        jacobian.value = np.array2string(J_G, formatter={'float': lambda x: \"{:5.2f}\".format(x)})\n",
        "        sigma.value = str(np.min(np.linalg.svd(J_G, compute_uv=False)))\n",
        "\n",
        "    meshcat.load()\n",
        "\n",
        "    # If you want to set the initial positions manually, use this:\n",
        "    # plant.SetPositions(plant.GetMyContextFromRoot(context),\n",
        "    #                   plant.GetModelInstanceByName(\"iiwa\"),\n",
        "    #                   [0, 0, 0, 0, 0, 0, 0])\n",
        "    # The default positions for this \"clutter clearing\" setup are:\n",
        "    #                   [-1.57, 0.1, 0, -1.2, 0, 1.6, 0]\n",
        "\n",
        "    MakeJointSlidersThatPublishOnCallback(station.get_multibody_plant(), meshcat, context, my_callback=pose_callback)\n",
        "    display(jacobian)\n",
        "    display(sigma)\n",
        "\n",
        "pick_and_place_jacobians_example()\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "GdbDWL4aQTlf",
        "colab_type": "text"
      },
      "source": [
        "# Teleop example from lecture 1\n",
        "\n",
        "Here is the example I gave you before.  Can you drive the Jacobian into singularity (like you might have done before).  Does it make more sense now?\n",
        "\n",
        "If you get stuck and need to restart, just press the \"Stop simulation\" button, then run the cell again.\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "JX1Oq7gfWsBD",
        "colab_type": "code",
        "tags": [],
        "colab": {}
      },
      "source": [
        "def teleop_example():\n",
        "    set_log_level(\"warn\")\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    station = builder.AddSystem(ManipulationStation())\n",
        "\n",
        "    station.SetupClutterClearingStation()\n",
        "    station.Finalize()\n",
        "\n",
        "    frames_to_draw = {\"gripper\": {\"body\"}}\n",
        "    meshcat = ConnectMeshcatVisualizer(builder,\n",
        "        station.get_scene_graph(),\n",
        "        output_port=station.GetOutputPort(\"pose_bundle\"),\n",
        "        delete_prefix_on_load=False,\n",
        "        zmq_url=zmq_url,\n",
        "        frames_to_draw=frames_to_draw,\n",
        "        axis_length=0.2,\n",
        "        axis_radius=0.005)\n",
        "\n",
        "    robot = station.get_controller_plant()\n",
        "    params = DifferentialInverseKinematicsParameters(robot.num_positions(),\n",
        "                                                    robot.num_velocities())\n",
        "\n",
        "    time_step = 0.005\n",
        "    params.set_timestep(time_step)\n",
        "    # True velocity limits for the IIWA14 (in rad, rounded down to the first\n",
        "    # decimal)\n",
        "    iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])\n",
        "    params.set_joint_velocity_limits((-iiwa14_velocity_limits,\n",
        "                                    iiwa14_velocity_limits))\n",
        "    differential_ik = builder.AddSystem(DifferentialInverseKinematicsIntegrator(\n",
        "        robot, robot.GetFrameByName(\"iiwa_link_7\"), time_step, params))\n",
        "    builder.Connect(differential_ik.get_output_port(),\n",
        "                    station.GetInputPort(\"iiwa_position\"))\n",
        "\n",
        "    teleop = builder.AddSystem(PoseSliders(\n",
        "        min_range = PoseSliders.MinRange(roll=0, pitch=-0.5, yaw=-np.pi, \n",
        "                                        x=-0.6, y=-0.8, z=0.0),\n",
        "        max_range = PoseSliders.MaxRange(roll=2*np.pi, pitch=np.pi, yaw=np.pi,\n",
        "                                        x=0.8, y=0.3, z=1.1)\n",
        "    ))\n",
        "    builder.Connect(teleop.get_output_port(0), \n",
        "                    differential_ik.get_input_port())\n",
        "    wsg_buttons = ToggleButtons(value=0.107, description=\"SchunkWsg\", \n",
        "                                options=[('Open', 0.107), ('Close', 0.002)])\n",
        "    wsg_teleop = builder.AddSystem(WidgetSystem([wsg_buttons]))\n",
        "    builder.Connect(wsg_teleop.get_output_port(0),\n",
        "                    station.GetInputPort(\"wsg_position\"))\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    simulator = Simulator(diagram)\n",
        "    context = simulator.get_mutable_context()\n",
        "\n",
        "    station_context = station.GetMyMutableContextFromRoot(context)\n",
        "\n",
        "    q0 = station.GetOutputPort(\"iiwa_position_measured\").Eval(\n",
        "        station_context)\n",
        "    differential_ik.get_mutable_parameters().set_nominal_joint_position(q0)\n",
        "    diff_ik_context = differential_ik.GetMyMutableContextFromRoot(context)\n",
        "    differential_ik.SetPositions(diff_ik_context, q0)\n",
        "    teleop.SetPose(differential_ik.ForwardKinematics(diff_ik_context))\n",
        "\n",
        "    if running_as_notebook:  # Then we're not just running as a test on CI.\n",
        "        simulator.set_target_realtime_rate(1.0)\n",
        "\n",
        "        stop_button = ToggleButton(value=False, description='Stop Simulation')\n",
        "        display(stop_button)\n",
        "        while not stop_button.value:\n",
        "            simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)\n",
        "        stop_button.value = False\n",
        "    \n",
        "    else:\n",
        "        simulator.AdvanceTo(0.1)\n",
        "\n",
        "teleop_example()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "tnUDKOyEbbMU",
        "colab_type": "text"
      },
      "source": [
        "# Our first end-effector \"controller\"\n",
        "\n",
        "Let's use the pseudo-inverse of the Jacobian to drive the robot around.  To do that, we'll write a very simple system that looks at the current value of $q$, computes $[J^G]^+$, and uses it to command a constant spatial velocity, $V^G$.\n",
        "\n",
        "We'll only run this controller for a short duration.  Constant spatial velocities aren't something that makes sense for a longer simulation!\n",
        "\n",
        "Make sure you try changing $V^G$, and understand how the command relates to the motion of the robot."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "H7llRg0MQx3t",
        "colab_type": "code",
        "tags": [],
        "colab": {}
      },
      "source": [
        "# We can write a new System by deriving from the LeafSystem class.\n",
        "# There is a little bit of boiler plate, but hopefully this example makes sense.\n",
        "class PseudoInverseController(LeafSystem):\n",
        "    def __init__(self, plant):\n",
        "        LeafSystem.__init__(self)\n",
        "        self._plant = plant\n",
        "        self._plant_context = plant.CreateDefaultContext()\n",
        "        self._iiwa = plant.GetModelInstanceByName(\"iiwa\")\n",
        "        self._G = plant.GetBodyByName(\"body\").body_frame()\n",
        "        self._W = plant.world_frame()\n",
        "\n",
        "        self.DeclareVectorInputPort(\"iiwa_position\", BasicVector(7))\n",
        "        self.DeclareVectorOutputPort(\"iiwa_velocity\", BasicVector(7), \n",
        "                                     self.CalcOutput)\n",
        "\n",
        "    def CalcOutput(self, context, output):\n",
        "        q = self.get_input_port().Eval(context)\n",
        "        self._plant.SetPositions(self._plant_context, self._iiwa, q)\n",
        "        J_G = self._plant.CalcJacobianSpatialVelocity(\n",
        "            self._plant_context, JacobianWrtVariable.kQDot, \n",
        "            self._G, [0,0,0], self._W, self._W)\n",
        "        J_G = J_G[:,0:7] # Ignore gripper terms\n",
        "        \n",
        "        V_G_desired = np.array([0,    # rotation about x\n",
        "                                -.1,  # rotation about y\n",
        "                                0,    # rotation about z\n",
        "                                0,    # x\n",
        "                                -.05, # y\n",
        "                                -.1]) # z\n",
        "        v = np.linalg.pinv(J_G).dot(V_G_desired)\n",
        "        output.SetFromVector(v)\n",
        "\n",
        "        \n",
        "def jacobian_controller_example():\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    station = builder.AddSystem(ManipulationStation())\n",
        "    station.SetupClutterClearingStation()\n",
        "    station.Finalize()\n",
        "\n",
        "    controller = builder.AddSystem(PseudoInverseController(\n",
        "        station.get_multibody_plant()))\n",
        "    integrator = builder.AddSystem(Integrator(7))\n",
        "\n",
        "    builder.Connect(controller.get_output_port(), \n",
        "                    integrator.get_input_port())\n",
        "    builder.Connect(integrator.get_output_port(),\n",
        "                    station.GetInputPort(\"iiwa_position\"))\n",
        "    builder.Connect(station.GetOutputPort(\"iiwa_position_measured\"),\n",
        "                    controller.get_input_port())\n",
        "\n",
        "    frames_to_draw = {\"gripper\": {\"body\"}}\n",
        "    meshcat = ConnectMeshcatVisualizer(builder,\n",
        "        station.get_scene_graph(),\n",
        "        output_port=station.GetOutputPort(\"pose_bundle\"),\n",
        "        delete_prefix_on_load=False,\n",
        "        zmq_url=zmq_url,\n",
        "        frames_to_draw=frames_to_draw,\n",
        "        axis_length=0.2,\n",
        "        axis_radius=0.005)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    display(SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString(max_depth=1))[0].create_svg()))\n",
        "    \n",
        "    simulator = Simulator(diagram)\n",
        "    station_context = station.GetMyContextFromRoot(simulator.get_mutable_context())\n",
        "    station.GetInputPort(\"iiwa_feedforward_torque\").FixValue(station_context, np.zeros((7,1)))\n",
        "    station.GetInputPort(\"wsg_position\").FixValue(station_context, [0.1])\n",
        "    # TODO(russt): Add this missing python binding\n",
        "    #integrator.set_integral_value(\n",
        "    #    integrator.GetMyContextFromRoot(simulator.get_mutable_context()), \n",
        "    #        station.GetIiwaPosition(station_context))\n",
        "    integrator.GetMyContextFromRoot(simulator.get_mutable_context()).get_mutable_continuous_state_vector().SetFromVector(station.GetIiwaPosition(station_context))\n",
        "\n",
        "    simulator.set_target_realtime_rate(1.0)\n",
        "    simulator.AdvanceTo(5.0 if running_as_notebook else 0.1)\n",
        "\n",
        "jacobian_controller_example()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "NAi5ayARbbMZ",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        ""
      ],
      "execution_count": null,
      "outputs": []
    }
  ]
}